using UnityEngine;

public class SoftWheelCollider : MonoBehaviour
{
    public LayerMask drivableSurface; // Only include these layers for collision detection

    public float springStrength, springDamper, radius, coefficientOfFriction = 0.5f;

    Rigidbody rb;
    [HideInInspector]
    public Collider[] ContactColliders;
    SphereCollider sphereCollider;
    [HideInInspector]
    public float NormalForce, compression;
    [HideInInspector]
    public Vector3 NormalDir;

    [HideInInspector]
    public Vector3 contactPoint;

    public float wheelWidth = 0.5f;


    void Start()
    {
        radius = GetComponent<SphereCollider>().bounds.extents.y;
        rb = GetComponent<Rigidbody>();
        rb.centerOfMass = Vector3.zero;
        rb.inertiaTensor = Vector3.one;
        sphereCollider = GetComponent<SphereCollider>();
        sphereCollider.isTrigger = true;
        //radius = sphereCollider.radius;
        //rb.angularVelocity = Vector3.one * 30;
    }

    void FixedUpdate()
    {

        ContactColliders = Physics.OverlapSphere(transform.position, radius, drivableSurface);

        if (ContactColliders.Length > 0)
        {
            ClosestPointOnMesh(ContactColliders[0], sphereCollider, out contactPoint, out NormalDir);
        }


        foreach (Collider contactCol in ContactColliders)
        {
            Vector3 pointOnMesh = Vector3.zero;
            Vector3 SurfaceNormal = Vector3.zero;


            ClosestPointOnMesh(contactCol, sphereCollider, out pointOnMesh, out SurfaceNormal);
            //contactPoint = pointOnMesh;
            //NormalDir = SurfaceNormal.normalized;

            if (contactCol.attachedRigidbody)
            {
                Rigidbody otherRigidbody = contactCol.attachedRigidbody;

                // Get the rigidbody of our object
                Rigidbody thisRigidbody = rb;

                // Calculate the velocity of our object just before the collision
                Vector3 thisVelocity = thisRigidbody.linearVelocity;

                // Calculate the velocity of the other object just before the collision
                Vector3 otherVelocity = otherRigidbody.linearVelocity;

                // Calculate the relative velocity between the two objects
                Vector3 relativeVelocity = thisVelocity - otherVelocity;

                // Calculate the normal of the collision
                Vector3 normal = SurfaceNormal;

                // Calculate the impulse of the collision
                float impulse = relativeVelocity.magnitude * otherRigidbody.mass;

                // Apply the impulse to our object in the direction of the normal
                otherRigidbody.AddForce(-normal * impulse, ForceMode.Impulse);
                thisRigidbody.AddForce(normal * impulse, ForceMode.Impulse);
                AddPointSpring(pointOnMesh, SurfaceNormal);
                AddFrictionOther(pointOnMesh, SurfaceNormal, otherRigidbody);

            }
            else
            {
                if (ContactColliders.Length > 0)
                {
                    AddFriction(pointOnMesh, SurfaceNormal);
                    AddPointSpring(pointOnMesh, SurfaceNormal);
                    //AddPressureForce(pointOnMesh, SurfaceNormal);
                }
            }

        }
        if (ContactColliders.Length < 1)
        {
            compression = Mathf.MoveTowards(compression, 0, 0.1f);
        }

    }

    public void AddFriction(Vector3 contactPoint, Vector3 surfaceNormal)
    {
        if (surfaceNormal == Vector3.zero)
        {
            Debug.LogWarning("Surface normal is zero! Cannot compute friction.");
            return;
        }

        Vector3 contactDistance = contactPoint - transform.position;
        Vector3 contactVel = rb.linearVelocity + Vector3.Cross(rb.angularVelocity, contactDistance);

        if (contactVel == Vector3.zero)
        {
            Debug.LogWarning("Contact velocity is zero! Friction calculation skipped.");
            return;
        }

        Vector3 contactDesiredAccel = -Vector3.ProjectOnPlane(contactVel, surfaceNormal) / Time.fixedDeltaTime;

        if (contactDesiredAccel.magnitude == 0 || float.IsNaN(contactDesiredAccel.magnitude))
        {
            Debug.LogWarning("Invalid desired acceleration! Skipping friction calculation.");
            return;
        }

        float minForce = Mathf.Min(
            rb.mass * Mathf.Abs(Physics.gravity.y) * 70 * coefficientOfFriction,
            NormalForce * coefficientOfFriction
        );

        Vector3 frictionForce = Vector3.ClampMagnitude(rb.mass * contactDesiredAccel, minForce);

        if (frictionForce == Vector3.zero || frictionForce.magnitude == 0)
        {
            Debug.LogWarning("Friction force is zero! No force applied.");
            return;
        }

        rb.AddForceAtPosition(frictionForce, contactPoint);
    }

    public void AddFrictionOther(Vector3 contactPoint, Vector3 SurfaceNormal, Rigidbody otherRb)
    {
        //Vector3 localVel = transform.InverseTransformDirection(rb.velocity);
        //Vector3 localAngVel = transform.InverseTransformDirection(rb.angularVelocity);
        Vector3 ContactDistance = contactPoint - transform.position;
        Vector3 contactVel = (otherRb.linearVelocity - rb.linearVelocity) + Vector3.Cross(otherRb.angularVelocity - rb.angularVelocity, ContactDistance);
        //Debug.DrawRay(contactPoint, contactVel.normalized, Color.gray);
        Vector3 contactDesiredAccel = -Vector3.ProjectOnPlane(contactVel, SurfaceNormal) / Time.fixedDeltaTime;
        //Vector3 frictionForce = contactDesiredAccel.normalized * Mathf.Clamp(rb.mass * contactDesiredAccel.magnitude, 0, 0.001f);
        Vector3 frictionForce = Vector3.ClampMagnitude(otherRb.mass * contactDesiredAccel, NormalForce * coefficientOfFriction);
        //Debug.DrawRay(contactPoint, frictionForce.normalized, Color.yellow);
        otherRb.AddForceAtPosition(frictionForce * (1 + compression), contactPoint);
    }

    void AddPointSpring(Vector3 vertexPos, Vector3 susfaceNormal)
    {
        float hitDistance = Vector3.Distance(transform.position, vertexPos);
        float sideDistance = Vector3.Dot(vertexPos - transform.position, transform.right * radius);
        float SideDeltaDistance = Mathf.Abs(sideDistance) - (wheelWidth / 2);
        float sideRatio = -(Mathf.Clamp01(SideDeltaDistance) / (radius - (wheelWidth / 2))) + 1;
        float sideMultiplier = Mathf.Abs(sideDistance) / radius;

        float offset = (radius - hitDistance) / radius;
        compression = Mathf.MoveTowards(compression, offset, 0.1f);
        float vel = Vector3.Dot(susfaceNormal.normalized, rb.linearVelocity);
        float force = (offset * springStrength * sideRatio) - (vel * springDamper);

        if (sideRatio < 0.9f)
        {
            //force = 0;
        }

        //float TotalMass = 200;
        //force = Mathf.Clamp(force, -TotalMass * Physics.gravity.magnitude, TotalMass * Physics.gravity.magnitude);
        //force = Mathf.Clamp(force, -Mathf.Abs(TotalMass * vel / Time.fixedDeltaTime), Mathf.Abs(TotalMass * vel / Time.fixedDeltaTime));
        NormalForce = Mathf.Abs(force);
        rb.AddForceAtPosition(susfaceNormal.normalized * force, vertexPos);

    }

    void AddPressureForce(Vector3 vertexPos, Vector3 susfaceNormal)
    {
        float hitDistance = Vector3.Distance(transform.position, vertexPos);

        float offset = radius - hitDistance;
        compression = Mathf.MoveTowards(compression, offset, 0.1f);
        float vel = Vector3.Dot(susfaceNormal.normalized, rb.linearVelocity);
        float F = Mathf.PI * springStrength * (radius * radius - (radius * radius - (offset * offset)));
        float D = (vel * springDamper);
        float force = F - D;
        NormalForce = Mathf.Abs(force);
        rb.AddForceAtPosition(susfaceNormal.normalized * force, vertexPos);

    }

    public void ClosestPointOnMesh(Collider target_collider, SphereCollider SphereCollider, out Vector3 closest_point, out Vector3 surface_normal)
    {
        closest_point = contactPoint;
        surface_normal = NormalDir;
        float surface_penetration_depth = 0;

        Vector3 Sphere_pos = transform.position;
        if (Physics.ComputePenetration(target_collider, target_collider.transform.position, target_collider.transform.rotation, SphereCollider, Sphere_pos, Quaternion.identity, out surface_normal, out surface_penetration_depth))
        {
            closest_point = Sphere_pos + (surface_normal * (SphereCollider.radius - surface_penetration_depth));

            surface_normal = -surface_normal;
        }
    }

    void createCylinderGizmos(float height, float radius)
    {
        Vector3 position = transform.position;

        UnityEditor.Handles.color = Color.green;
        UnityEditor.Handles.DrawWireDisc(position + (transform.right * height / 2), transform.right, radius);
        UnityEditor.Handles.DrawWireDisc(position - (transform.right * height / 2), transform.right, radius);
        UnityEditor.Handles.DrawLine(position + (transform.right * height / 2) + transform.up * radius,
            position - (transform.right * height / 2) + transform.up * radius);
        UnityEditor.Handles.DrawLine(position + (transform.right * height / 2) - transform.up * radius,
            position - (transform.right * height / 2) - transform.up * radius);
        UnityEditor.Handles.DrawLine(position + (transform.right * height / 2) + transform.forward * radius,
            position - (transform.right * height / 2) + transform.forward * radius);
        UnityEditor.Handles.DrawLine(position + (transform.right * height / 2) - transform.forward * radius,
            position - (transform.right * height / 2) - transform.forward * radius);
    }

    private void OnDrawGizmos()
    {
        Gizmos.color = Color.red;
        Gizmos.DrawSphere(transform.position, 0.05f);

        if (Application.isPlaying)
        {
            Gizmos.color = Color.blue;
            //Gizmos.DrawRay(transform.position, rb.velocity.normalized);
            Gizmos.color = Color.red;
            //Gizmos.DrawRay(transform.position, rb.angularVelocity.normalized);
            Gizmos.color = Color.cyan;
            foreach (Collider contactCol in ContactColliders)
            {
                Vector3 pointOnMesh = Vector3.zero;
                Vector3 SurfaceNormal = Vector3.zero;
                ClosestPointOnMesh(contactCol, sphereCollider, out pointOnMesh, out SurfaceNormal);
                Gizmos.DrawSphere(pointOnMesh, 0.05f);
                Gizmos.DrawLine(transform.position, pointOnMesh);

            }
        }


        createCylinderGizmos(wheelWidth, radius);
    }
}
